// --------------------------------------------------------------------------------------------------------------------
// <copyright file="RunTimeMessageTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Manipulation
{
    using System;
    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Test manipulation run time message creation
    /// </summary>
    [TestClass]
    public class RunTimeMessageTest
    {

        /// <summary>
        /// Test to create a joint control message
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationArmJointControlMessageTest()
        {
            double[] jointAngles = new double[KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS];
            double maxTol = 0.1;

            for (int i = 0; i < jointAngles.Length; ++i)
            {
                jointAngles[i] = 2.0 * i;
            }

            JointGoalPositionControlMessage msg = new JointGoalPositionControlMessage(AgentMessage.GetCurrentTime(), jointAngles, maxTol);
            Assert.IsNotNull(msg);

            for (int i = 0; i < jointAngles.Length; ++i)
            {
                Assert.IsTrue(Math.Abs(jointAngles[i]- msg.ArmJointPositions[i]) < MathConstants.ErrorEpsilon, "Armjointposition mismatch");
            }

            Assert.IsTrue(Math.Abs(maxTol - msg.MaximimumTolerance) < MathConstants.ErrorEpsilon, "Maximum tolerance mismatch");
        }

        /// <summary>
        /// Test to create an arm stop message
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationArmStopMessage()
        {
            ArmStopMessage msg = new ArmStopMessage(AgentMessage.GetCurrentTime());
            Assert.IsNotNull(msg);
        }

        /// <summary>
        /// Test to create a LinearCartesianPushControlMessage
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationLinearCartesianPushControlMessage()
        {
            try
            {
                Pose pose = new Pose(new Vector3(0.1, 0.1, 0.1));
                double maxLinearSpeed = 0.1;
                double maxRotationSpeed = 0.05;
                Vector3 pushDirection = new Vector3(1, 0, 0);
                double pushForceThreshold = 2.0;
                double residualForceThreshold = 1.2;

                LinearCartesianPushControlMessage msg = new LinearCartesianPushControlMessage(
                            AgentMessage.GetCurrentTime(),
                            pose,
                            maxLinearSpeed,
                            maxRotationSpeed,
                            pushDirection,
                            pushForceThreshold,
                            residualForceThreshold);

                Assert.IsNotNull(msg, "Message creation failure");
                Assert.IsTrue(pose == msg.CartesianPosition, "pose mismatch");
                Assert.IsTrue(Math.Abs(maxLinearSpeed - msg.MaxLinearCartesianSpeedInMetersPerSecond) < MathConstants.ErrorEpsilon, "Max linear speed mismatch");
                Assert.IsTrue(Math.Abs(maxRotationSpeed - msg.MaxRotationalCartesianSpeedInRadiansPerSecond) < MathConstants.ErrorEpsilon, "Max Rotation speed mismatch");
                Assert.IsTrue(Math.Abs(pushForceThreshold - msg.PushForceThresholdInNewtons) < MathConstants.ErrorEpsilon, "PushForceThreshold mismatch");
                Assert.IsTrue(Math.Abs(residualForceThreshold - msg.ResidualForceThresholdInNewtons) < MathConstants.ErrorEpsilon, "ResidualForceThreshold mismatch");
                Assert.IsTrue(pushDirection == msg.PushDirection, "push direction mismatch");
            }
            catch (ArgumentException)
            {
                Assert.IsTrue(false, "Exception while creating method");
            }
        }


        /// <summary>
        /// Test to check failure conditions on LinearCartesianPushControlMessage creation
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationLinearCartesianPushControlMessageFail()
        {
            try
            {
                LinearCartesianPushControlMessage msg = new LinearCartesianPushControlMessage(
                            AgentMessage.GetCurrentTime(),
                            new Pose(new Vector3(0.1, 0.1, 0.1)),
                            -0.1,
                            0.05,
                            new Vector3(1, 0, 0),
                            2.0,
                            0.1);

                Assert.IsNull(msg, "Expect exception to catch negative linear speed");
            }
            catch (ArgumentException exc)
            {
                Assert.IsNotNull(exc, "Expect exception - Negative maximum linear speed");
            }

            try
            {
                LinearCartesianPushControlMessage msg = new LinearCartesianPushControlMessage(
                            AgentMessage.GetCurrentTime(),
                            new Pose(new Vector3(0.1, 0.1, 0.1)),
                            0.1,
                            -0.05,
                            new Vector3(1, 0, 0),
                            2.0,
                            0.1);

                Assert.IsNull(msg, "Expect exception to catch negative rotation speed");
            }
            catch (ArgumentException exc)
            {
                Assert.IsNotNull(exc, "Expect exception - Negative maximum rotation speed");
            }

            try
            {
                LinearCartesianPushControlMessage msg = new LinearCartesianPushControlMessage(
                            AgentMessage.GetCurrentTime(),
                            new Pose(new Vector3(0.1, 0.1, 0.1)),
                            0.1,
                            0.05,
                            new Vector3(1, 0, 0),
                            -2.0,
                            0.1);

                Assert.IsNull(msg, "Expect exception to catch negative push force threshold");
            }
            catch (ArgumentException exc)
            {
                Assert.IsNotNull(exc, "Expect exception - Negative push force threshold");
            }

            try
            {
                LinearCartesianPushControlMessage msg = new LinearCartesianPushControlMessage(
                            AgentMessage.GetCurrentTime(),
                            new Pose(new Vector3(0.1, 0.1, 0.1)),
                            0.1,
                            0.05,
                            new Vector3(1, 0, 0),
                            2.0,
                            -0.1);

                Assert.IsNull(msg, "Expect exception to catch residual force threshold");
            }
            catch (ArgumentException exc)
            {
                Assert.IsNotNull(exc, "Expect exception - Negative residual force threshold");
            }

            try
            {
                LinearCartesianPushControlMessage msg = new LinearCartesianPushControlMessage(
                            AgentMessage.GetCurrentTime(),
                            new Pose(new Vector3(0.1, 0.1, 0.1)),
                            0.1,
                            0.05,
                            new Vector3(1, 1, 0),
                            2.0,
                            0.1);

                Assert.IsNull(msg, "Expect exception to catch unit vector push direction");
            }
            catch (ArgumentException exc)
            {
                Assert.IsNotNull(exc, "Expect exception - unit push direction vector");
            }
        }

        /// <summary>
        /// Test to create a LinearCartesianPushControlMessage
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationLinearCartesianTouchControlMessage()
        {
            try
            {
                Pose pose = new Pose(new Vector3(0.1, 0.1, 0.1));
                double maxLinearSpeed = 0.1;
                double maxRotationSpeed = 0.05;
                double forceThreshold = 2.0;

                LinearCartesianTouchControlMessage msg = new LinearCartesianTouchControlMessage(
                            AgentMessage.GetCurrentTime(),
                            pose,
                            maxLinearSpeed,
                            maxRotationSpeed,
                            forceThreshold);

                Assert.IsNotNull(msg, "Message creation failure");
                Assert.IsTrue(pose == msg.CartesianPosition, "pose mismatch");
                Assert.IsTrue(Math.Abs(maxLinearSpeed - msg.MaxLinearCartesianSpeedInMetersPerSecond) < MathConstants.ErrorEpsilon, "Max linear speed mismatch");
                Assert.IsTrue(Math.Abs(maxRotationSpeed - msg.MaxRotationalCartesianSpeedInRadiansPerSecond) < MathConstants.ErrorEpsilon, "Max Rotation speed mismatch");
                Assert.IsTrue(Math.Abs(forceThreshold - msg.ForceThresholdInNewtons) < MathConstants.ErrorEpsilon, "PushForceThreshold mismatch");
            }
            catch (ArgumentException)
            {
                Assert.IsTrue(false, "Exception while creating method");
            }

        }


        /// <summary>
        /// Test LinearCartesianPushControlMessage creation failure conditions
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationLinearCartesianTouchControlMessageFail()
        {
            try
            {
                LinearCartesianTouchControlMessage msg = new LinearCartesianTouchControlMessage(
                            AgentMessage.GetCurrentTime(),
                            new Pose(new Vector3(0.1, 0.1, 0.1)),
                            -0.1,
                            0.05,
                            2.1);

                Assert.IsNull(msg, "Expect exception to catch negative linear speed");
            }
            catch (ArgumentException exc)
            {
                Assert.IsNotNull(exc, "Expect exception - Negative maximum linear speed");
            }

            try
            {
                LinearCartesianTouchControlMessage msg = new LinearCartesianTouchControlMessage(
                            AgentMessage.GetCurrentTime(),
                            new Pose(new Vector3(0.1, 0.1, 0.1)),
                            0.1,
                            -0.05,
                            2.1);

                Assert.IsNull(msg, "Expect exception to catch negative rotation speed");
            }
            catch (ArgumentException exc)
            {
                Assert.IsNotNull(exc, "Expect exception - Negative maximum rotation speed");
            }

            try
            {
                LinearCartesianTouchControlMessage msg = new LinearCartesianTouchControlMessage(
                            AgentMessage.GetCurrentTime(),
                            new Pose(new Vector3(0.1, 0.1, 0.1)),
                            0.1,
                            0.05,
                            -2.1);

                Assert.IsNull(msg, "Expect exception to catch negative force threshold");
            }
            catch (ArgumentException exc)
            {
                Assert.IsNotNull(exc, "Expect exception - Negative force threshold");
            }
        }

        /// <summary>
        /// Test to create a LinearCartesianPushControlMessage
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationLinearCartesianMotionControlMessage()
        {
            Pose pose = new Pose(new Vector3(0.1, 0.1, 0.1));
            double maxTol = 0.01;

            LinearCartesianMotionControlMessage msg = new LinearCartesianMotionControlMessage(
                AgentMessage.GetCurrentTime(),
                 pose,
                 maxTol);

            Assert.IsTrue(pose == msg.CartesianPosition, "pose mismatch");
            Assert.IsTrue(Math.Abs(maxTol - msg.MaximimumTolerance) < MathConstants.ErrorEpsilon, "Max tolerance mismatch");
        }

        /// <summary>
        /// Test to create a CartesianGoalMotionControlMessage
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationCartesianMotionControlMessage()
        {
            Pose pose = new Pose(new Vector3(0.1, 0.1, 0.1));
            double translationalTol = 0.01;
            double rotationalTol = 0.02;

            CartesianGoalPositionControlMessage msg = new CartesianGoalPositionControlMessage(
                AgentMessage.GetCurrentTime(),
                 pose,
                 translationalTol,
                 rotationalTol);

            Assert.IsTrue(pose == msg.CartesianPosition, "pose mismatch");
            Assert.IsTrue(Math.Abs(translationalTol - msg.TranslationalTolerance) < MathConstants.ErrorEpsilon, "Translational tolerance mismatch");
            Assert.IsTrue(Math.Abs(rotationalTol - msg.RotationalTolerance) < MathConstants.ErrorEpsilon, "Rotational tolerance mismatch");
        }
    }

}
